#include "math_utils.h"

// 对超过 0-2PI 的角度转为 0-2PI
double rad2mrad(double v) {
    v = fmod(v, 2 * PI);
    if (v < 0) return v + 2 * PI;
    return v;
}

// 对超过 -PI 到 PI 的角度转为 -PI 到 PI
double rad2rrad(double v) {
    v = fmod(v, 2 * PI);
    if (v <= -PI) return v + 2 * PI;
    if (v > PI) return v - 2 * PI;
    return v;
}

// 临界余数（a 与最近的整倍数 b 相差的距离）
double mod2(double a, double b) {
    double c = fmod(a + b, b);
    if (c > b / 2.0) c -= b;
    return c;
}

// 球面转直角坐标
void llr2xyz(const double JW[3], double result[3]) {
    double J = JW[0], W = JW[1], R = JW[2];
    result[0] = R * cos(W) * cos(J);
    result[1] = R * cos(W) * sin(J);
    result[2] = R * sin(W);
}

// 直角坐标转球面
void xyz2llr(const double xyz[3], double result[3]) {
    double x = xyz[0], y = xyz[1], z = xyz[2];
    result[2] = sqrt(x * x + y * y + z * z);
    result[1] = asin(z / result[2]);
    result[0] = rad2mrad(atan2(y, x));
}

// 球面坐标旋转
void llrConv(const double JW[3], double E, double result[3]) {
    double J = JW[0], W = JW[1];
    result[0] = atan2(sin(J) * cos(E) - tan(W) * sin(E), cos(J));
    result[1] = asin(cos(E) * sin(W) + sin(E) * cos(W) * sin(J));
    result[2] = JW[2];
    result[0] = rad2mrad(result[0]);
}

// 赤道坐标转为地平坐标
void CD2DP(const double z[3], double L, double fa, double gst, double result[3]) {
    double a[3] = {z[0] + PI / 2 - gst - L, z[1], z[2]};
    llrConv(a, PI / 2 - fa, result);
    result[0] = rad2mrad(PI / 2 - result[0]);
}

// 求角度差
double j1_j2(double J1, double W1, double J2, double W2) {
    double dJ = rad2rrad(J1 - J2), dW = W1 - W2;
    if (fabs(dJ) < 1 / 1000.0 && fabs(dW) < 1 / 1000.0) {
        dJ *= cos((W1 + W2) / 2);
        return sqrt(dJ * dJ + dW * dW);
    }
    return acos(sin(W1) * sin(W2) + cos(W1) * cos(W2) * cos(dJ));
}

// 日心球面转地心球面
void h2g(const double z[3], const double a[3], double result[3]) {
    double a_xyz[3], z_xyz[3];
    llr2xyz(a, a_xyz); // 地球
    llr2xyz(z, z_xyz); // 星体
    z_xyz[0] -= a_xyz[0];
    z_xyz[1] -= a_xyz[1];
    z_xyz[2] -= a_xyz[2];
    xyz2llr(z_xyz, result);
}

// 视差角（不是视差）
double shiChaJ(double gst, double L, double fa, double J, double W) {
    double H = gst + L - J; // 天体的时角
    return rad2mrad(atan2(sin(H), tan(fa) * cos(W) - sin(W) * cos(H)));
}